'''
Created on 7 nov 2009

@author: fredrik
'''
#######################################################################
#                  Demonstration of the A* Algorithm                  #
#######################################################################
# Copyright (C) 2009 Michael Vandborg                                 #
# Email: michael@vandb.org                                            #
# Website: http://vandb.org                                           #
#######################################################################
#This program is free software: you can redistribute it and/or modify #
#it under the terms of the GNU General Public License as published by #
#the Free Software Foundation, either version 3 of the License, or    #
#(at your option) any later version.                                  #
#                                                                     #
#This program is distributed in the hope that it will be useful,      #
#but WITHOUT ANY WARRANTY; without even the implied warranty of       #
#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the        #
#GNU General Public License for more details.                         #
#                                                                     #
#You should have received a copy of the GNU General Public License    #
#along with this program.  If not, see <http://www.gnu.org/licenses/>.#
#######################################################################

class Node(object):
    __slots__ = ('f','g','h','blocked','parent','coord')
    def __init__(self, f = 0, g = 0, h = 0, blocked = False, parent = None):
        self.f = f
        self.g = g
        self.h = h
        self.blocked = blocked
        self.parent = None
        self.coord = None

    def __cmp__(self, other):
        return cmp(self.f, other.f)

class PathSearchLimitException(Exception):
    pass

from heapq import heappop,heappush

class Astar:
    def __init__(self, nodes, max_w, max_h,search_limit=None):
        self._nodes = nodes
        self.max_w, self.max_h = max_w, max_h
        self.target = None
        self._searched_nodes = set()
        self.search_limit = search_limit
    
    def add_node_open(self, node, x, y, parent=None):
        node.coord = (x, y)
        node.parent = parent
        self._openset.add(node.coord)
        heappush(self._open,node)

    def dist_between(self, node, parent, diagonal):
        if diagonal:
            return 14
        else:
            return 10

    def reset(self):
        self._open = []
        self._openset = set()
        self._closed = {}
        # reset the nodes touched in by last find path
        for n in self._searched_nodes:
            n.f = n.g = n.h = 0
        self._searched_nodes = set()
        
    def find_path(self,(x_start, y_start), (x_target, y_target)):
        self.reset()
        self._set_start(x_start, y_start)
        self._set_target(x_target, y_target)
        start_node = self._open[0]
        start_node.g = 0
        x,y = start_node.coord
        start_node.h = self.heuristic(x, y)
        
        while self._open:
            if (self.search_limit is not None) and self.search_limit < len(self._searched_nodes):
                raise PathSearchLimitException('Search limit reached:%d'%(len(self._searched_nodes),))
            node = heappop(self._open)
            if node.coord == self.target.coord:
                print "Path found, nodes searched", len(self._searched_nodes)
                return self.reconstruct_path(node)
            
            self._openset.discard(node.coord)
            self._closed[node.coord] = node
            
            self.inspect_adjacent(node)
        print "No Path found, nodes searched", len(self._searched_nodes)
        return None
    
    def heuristic(self, x, y):
        #hamiltonian path
        target_x,target_y = self.target.coord
        return abs(target_x - x) * 10 + abs(target_y - y) * 10
    
    def inspect_adjacent(self, node):
        x,y = node.coord
        n = self._nodes

        l = r = t = b = False
        if x > 0: #left
            l = self.inspect_node(x-1, y, node)
        if x < self.max_w - 1: #right
            r = self.inspect_node(x+1, y, node)
        if y > 0: #top
            t = self.inspect_node(x, y-1, node)
        if y < self.max_h - 1: #bottom
            b = self.inspect_node(x, y+1, node)

        # Diagonal path search 
        # special for turb game.
        # only allow diagonal moves if no blocking tiles are near 
        # as we will crash in the corner else.
        if (l and t) and x > 0 and y > 0: #top-left
            self.inspect_node(x-1, y-1, node, True)
        if (r and t) and x < self.max_w - 1 and y > 0: #top-right
            self.inspect_node(x+1, y-1, node, True)
        if (l and b) and x > 0 and y < self.max_h - 1: #bottom-left
            self.inspect_node(x-1, y+1, node, True)
        if (l and b) and x < self.max_w - 1 and y < self.max_h - 1: #bottom-right
            self.inspect_node(x+1, y+1, node, True)
    
    def inspect_node(self, x, y, parent, diagonal=False):
        node = self._nodes[x][y]
        self._searched_nodes.add(node)
        
        if node.coord in self._closed:
            return
        
        if not node.blocked:
            better = False
            new_g = parent.g + self.dist_between(node, parent, diagonal)
            
            if not node.coord in self._openset:
                node.parent = parent
                node.g = new_g
                node.h = self.heuristic(x, y)
                node.f = node.g + node.h
                self.add_node_open(node, x, y, parent)
                better = True
            elif new_g < node.g:
                node.parent = parent
                node.g = new_g
                node.h = self.heuristic(x, y)
                node.f = node.g + node.h
                better = True
            return True
        else:
            # node is blocked
            return False
                

    def reconstruct_path(self, node):
        path = []
        loop_node = node
        while loop_node.parent:
            path.append(loop_node)
            loop_node = loop_node.parent
        return path
        
    def _set_start(self, x, y):
        self._open = []
        self._openset = set()
        self.add_node_open(self._nodes[x][y], x, y)
    
    def _set_target(self, x, y):
        self.target = self._nodes[x][y]
        self.target.coord = (x, y)


def print_route(nodes,route):
    res = []
    route = set(id(n) for n in route)
    for x in range(len(nodes)):
        cols = []
        for y in range(len(nodes[x])):
            if id(nodes[x][y]) in route:
                cols.append('*')
            else:
                cols.append(str(int(nodes[x][y].blocked)))
        res.append(''.join(cols))
    print '\n'.join(res)

def test():
    MAP_DATA = '''
    00010000000100000000
    10010000000000000000
    10000000000001000000
    10010000001000000000
    00010100000000000000
    00000010100000000000
    10110010110010011010
    00000010000010000000
    00000010000000000000
    00000010000010000000
    00000010000000000000
    '''
    import time
    nodes = []
    expand = 7
    nodes = [ [Node(blocked=bool(int(y))) for y in x.strip()*expand] for x in MAP_DATA.strip().split('\n')*expand]
    
    start_time = time.time()
    astar = Astar(nodes, len(nodes), len(nodes[0]),search_limit = 2000)
    for i in range(10):
        route = astar.find_path((2,1), (10*expand,19*expand))
        print "calced"
    end_time = time.time()
    print_route(nodes,route)
    print "opt version"
    print "route time:",end_time-start_time
    print "Route length",len(route)

if __name__ == '__main__':
    profile = False
    if profile:
        import cProfile
        cProfile.run('test()')
    else:
        test()

